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Abstract. This paper develops different discretization schemes for nonholo- 
nomic mechanical systems through a discrete geometric approach. The pro- 
posed methods are designed to account for the special geometric structure of 
the nonholonomic motion. Two different families of nonholonomic integrators 
are developed and examined numerically: the geometric nonholonomic inte- 
grator (GNI) and the reduced d'Alembert-Pontryagin integrator (RDP). As a 
result, the paper provides a general tool for engineering applications, i.e. for 
automatic derivation of numerically accurate and stable dynamics integration 
schemes applicable to a variety of robotic vehicle models. 



1. Introduction 

Nonholonomic constraints have been the subject of deep analysis since the dawn 
of Analytical Mechanics. Hertz, in 1894, was the first to use the term "nonholo- 
nomic system", but we can even find older references in the work by Euler in 1734, 
who studied the dynamics of a rolling rigid body moving without slipping on a 
horizontal plane. Many authors have recently shown a new interest in that theory 
and also in its relation to the new developments in control theory, subriemannian 
geometry, robotics, etc (see, for instance, [24]). The main characteristic of this 
period is that Geometry was used in a systematic way (see L.D. Fadeev and A.M. 
Vershik [28] as an advanced and fundamental reference, and also, [1, 2, 5, 9, 18, 20] 
and references therein). 

In the case of nonholonomic mechanics, these constraint functions are, roughly 
speaking, functions on the velocities that are not derivable from position con- 
straints. Traditionally, the equations of motion for nonholonomic mechanics arc 
derived from the Lagrange-d Alembert principle which restricts the set of infinites- 
imal variations (or constrained forces) in terms of the constraint functions. 

Recent works, such as [8, 10, 14, 23], have introduced numerical integrators for 
nonholonomic systems with very good energy behavior and properties such as the 
preservation of the discrete nonholonomic momentum map. In this paper, we will 
review and compare two new methods for nonholonomic mechanics, the Geometric 
Nonholonomic Integrator (GNI) [12] and the Reduced dAlembert-Pontryagin In- 
tegrator (RDP) [17], examining their behavior in the numerical simulation of some 
of the most typical examples in nonholonomic mechanics: the Chaplygin sleigh and 
the snakeboard. 

Finally, the developed algorithms are packaged as a general computational tool 
for automatic derivation of nonholonomic integrators given the system constraints 
and Lagrangian. It is available for download from 
http : //www. cds . caltech. edu/ ~mar in/ index .php?n=nh.i 
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2. Introduction to Discrete Mechanics 

Discrete variational integrators appear as a special kind of geometric integrators 
(see [13, 27]). These integrators have their roots in the optimal control literature in 
the 1960's and 1970's. In the sequel we will review the construction of this specific 
type of geometric integrators (see [22] for an excellent survey about this topic). 

A discrete Lagrangian is a map L d : Q x Q — > R, where Q is a finite-dimensional 
configuration manifold. For the construction of numerical integrators for a continu- 
ous Lagrangian system given by a Lagrangian L : TQ — > R, the discrete Lagrangian 
may be considered as an approximation of the integral action 

L d (q ,qi)~ [ L(q(t),q(t))dt 



JO 

where q(t) is a solution of the Euler-Lagrange equations corresponding to L, that 
is, 

(1) Jt(^ iq{t) > m ) -^(9(*)'9(*))=°. 

additionally satisfying g(0) = go and q(h) = q\, where h is the time step. Observe 
that this solution always exists if the Lagrangian is regular and h is small enough 
(see [25]). 

Define the action sum S d : Q N+1 — > R corresponding to the Lagrangian Ld by 

N 

Sd = Y^ L d(qk-i,qk), 

k=l 

where qk € Q for < k < N. For any covector a £ T? X2 )(Q x Q)i we have a 
decomposition a = a\ + ai where ai E T*.Q. Therefore, 

dL d (qo,qi) = D 1 L d {q ,q 1 ) + D 2 L d (q ,q 1 ) . 

The discrete variational principle states that the solutions of the discrete system 
determined by L d must extremize the action sum given fixed points qo and q^. 

Extremizing Sd over qk-, 1 < k < N — 1, we obtain the following system of 
difference equations 

(2) D 1 L d (q k ,q k+1 )+D 2 L d (q k - 1 ,q k ) =0 . 

These equations are usually called the discrete Euler-Lagrange equations. 

The geometrical properties corresponding to this numerical method are obtained 
defining two discrete Legendre transformations associated to L d by 

¥-L d : QxQ — > T*Q 

(q ,qi) i — > (q Q ,~D 1 L d (q a ,q 1 )) 

¥+L d : QxQ — > T*Q 

(q ,qi) i — > (q( ) ,D 2 L d (qQ,q 1 )) 

and the 2-form ui d = (F ± L c ;)*^g, where ujq is the canonical symplectic form on 
T*Q. We will say that the discrete Lagrangian is regular if F~ L d is a local diffeo- 
morphism. We will have that: 

F~ L d is a local diffcomorphism <^> ¥ + L d is a local diffcomorphism 

ui d is symplectic 
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Under this regularity condition, this implicit system of difference equations (2) 
defines a local discrete flow T : U C Q X Q — > Q x Q, by T(qk-i,qk) = {qk, 1k+i)- 
The discrete algorithm determined by T preserves the symplcctic form Ud, i.e., 
T*uj c i = Ud- Moreover, if the discrete Lagrangian is invariant under the diagonal 
action of a Lie group G, then the discrete momentum map J c i : Q x Q — > q* defined 
by (Jd{Qk,Qk+i),0 = {D2Ld{qk,qk+i),£,Q{<lk+i)) is preserved by the discrete flow. 
Here, £q denotes the fundamental vector field determined by £ € g: 

&3(<7) = i (exp(iO-g). 
at t=o 

Therefore, these integrators are symplectic-momentum preserving integra- 
tors. 

In [21] we have obtained a geometric derivation of variational integrators that is 
also valid for reduced systems (on Lie algebras, quotient of tangent bundles by a 
Lie group action, etc.) 



3. Description of the nonholonomic dynamics 

The presence of nonholonomic (or holonomic) constraints gives rise to forces. 
Nonholonomic systems are described by the Lagrange-D'Alembert's principle which 
prescribes the constraint forces induced by the given nonholonomic constraints. In 
the following we will describe the equations of motion of a nonholonomic system in 
terms of Riemannian geometric tools (see [5]). 

Let Q be an ?i-dimensional differentiable manifold, with local coordinates (q l ), 
1 < i < n. Consider a mechanical Lagrangian system L : TQ M. defined by 
L(v q ) = \Q(v q ,v q ) - V(q), v q e T q Q or, locally 

(3) L(q,q) = ±g tJ (q)q*qi -V(q). 

Here Q is a Riemannian metric on Q (locally defined by the symmetric, positive 
definite matrix (gij(q))i<i,j< n ) and V represents a potential function. We know 
that the equations of motion for a Lagrangian system are (1) which, in the case of 
a mechanical Lagrangian system of the form (3), admits a nice expression in terms 
of standard Ricmmanian geometric tools: 

Vc W c(t) = -grad V{c{t)) 

where V is the Levi-Civita connection associated to Q and, in coordinates, grad V(c(t)) = 
9^ W7 wn ere (g 1 -*) is the inverse matrix of (<7y). 

Assume that the system is subjected to nonholonomic constraints, defined by 
a regular distribution V on Q, with rank V = n — m. Locally the nonholonomic 
constraints are described by the vanishing of m independent functions 

4> a = Hi(q)q l , 1 < a < m (the "constraint functions"). 

The Lagrange-d'Alembert principle states that the equations of motion for a non- 
holonomic system determined by the two data {L,V) are: 

(4) j t (^(3(t), «(*))) - ^(q(t),m) - *ari(q(t)) , 

^{q{t))cf{t)=Q 



4 



M. KOBILAROV, D. MARTIN DE DIEGO, AND S. FERRARO 



where A Q , 1 < a < m are Lagrange multipliers to be determined. Using the Levi- 
Civita connection we find an intrinsic equation for the nonholonomic equations: 

V 4 ( t )c(t) = -grad V(c(t)) + X(t), c(t) E V c{t) , 

where A is a section of T>^ along c. Here T> stands for the orthogonal complement 
of V with respect to the metric Q. 

In coordinates, defining the n 3 functions (Christoffel symbols for V) by 

V 9 =T k 9 
dqi ij dq k ' 

we may rewrite the nonholonomic equations of motion as 

q k {t)+T^(c(t))q l (t)qi(t) = -g k \c{t))— i+ \ a {t)g^(c(t))^{c{t)) , 
Ht{c{t))q i {t) = Q. 

4. Geometric Nonholonomic Integrator - GNI 

Given a nonholonomic system (L, T>) where L is a Lagrangian system of mechan- 
ical type (3), using the metric G, we may consider the complementary projectors 

Q: TQ^V^ ^TQ 

and their duals considered as mappings from T*Q to T*Q. 

The Geometric Nonholonomic integrator (GNI, in the sequel) for a nonholonomic 
system only needs to fix a discrete Lagrangian Ld : Q x Q — > M to derive a numerical 
scheme, that is, it is not necessary to discretize the nonholonomic constraints for 
this type of integrator. The discrete nonholonomic equations proposed in [12] 
are 

(5a) V^DiL^qu+i^+V^DiL^qk-uqk)) = 

(5b) C* w (I>iAj(<fc,gfc+i)) - Q* qk (D 2 L d (q k ^ liqk )) = 0. 

The first equation is the projection of the discrete Euler-Lagrange equations to the 
dual of the constraint distribution D, while the second one can be interpreted as 
an elastic impact of the system against T>. This defines a unique discrete evolution 
operator if and only if the Lagrangian Ld is regular, in the sense of Section 2. 
Define the pre- and post-momenta using the discrete Legendre transformations: 

Pk-i,k =^ + L d (qk-i,qk) = (qk,D 2 Ld(qk-i,qk)) G T* k Q 
Pk,k+i =^~L d {qk,qk+i) = (qk,-D 1 L d (qk,qk+i)) ^T* k Q. 
In these terms, equation (5b) can be rewritten as 

~* ( Pk,k+i +Pfe-i,fc \ _ „ 
^ 2 ) ° 

which means that the average of post- and prc-momenta satisfies the nonholonomic 
constraints. 

We can also rewrite the discrete nonholonomic equations as a jump of momenta: 

(6) Pw = (P*-Q*)\Jpt-i, k )- 
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Reversibility. Note that the map S = V* — Q* is an involution, that is S^ 1 = S. 
Therefore, it acts equivalently in both directions, i.e. it creates a reversible and 
symmetric flow. Furthermore, it can be expressed as 

S{q) = U(q)DU- 1 {q), 

where D is a diagonal matrix with elements ±1 corresponding to the eigenvalues 
of S while U is an invertible matrix with columns the eigenvectors of S. Thus, the 
update (6) can be written as 

(7) U-\ qk )p Kk+1 = DU-\q k )pt^ k 

based on which one can regard the momentum as either remaining unchanged (cor- 
responding to +1 eigenvalues) or being reflected (corresponding to —1 eigenvalues) 
with respect to the basis defined by the mapping U . 

Preservation Properties. Suppose that Q is a manifold on which a Lie group G acts. 
Define for each q g Q 

(8) 9 = {£ €01^(9)6^}, 

where £q (q) is the infinitesimal generator vector field corresponding to £ G g at the 
point q. The bundle over Q whose fiber at q is g q is denoted by g v . Define the 
discrete nonholonomic momentum map J^ h : Q x Q — > (q )* as in [8] by 

£ (->■ (D 2 L d (q k - 1 ,q k ),^Q(q k )) . 

For any smooth section £ of g 23 we have a function (JJ h )^: Q x Q -> 1, defined as 

(Jf = J2 h (Qk-uq k ) (&?*))• 

If id is G- invariant and £ € g is a horizontal symmetry (that is, £q((?) £ P g for 
all g G Q), then the GNI preserves (J^ 1 )^ (see [12] for a proof). 

In some cases of interest, it is possible to obtain an integrator preserving energy 
applying the following theorem (see [12]): 

Theorem 1. Let the configuration manifold be a Lie group with a bi-invariant 
Lagrangian and with an arbitrary distribution T>, and take a discrete Lagrangian 
that is left- invariant. Then the GNL (5) is energy-preserving. 

4.1. Nonholonomic version of the RATTLE and SHAKE methods. Con- 
sider a continuous nonholonomic system determined by the mechanical Lagrangian 
L : K 2 " -> M: 

L(q,q) = ^q T Mq-V(q) 

(with M a constant, invertible matrix) and the constraints determined by fi(q)q = 
where /i(q) is a m X n matrix with rank \i = m. 
Consider now the symmetric discretization 

T , v 1 , T ( qk+i -qk\ . i , T ( qk+i - qk 

L d {q k ,q k+1 ) = -hL \q k , I + -hL \q k +i, 7 

= ^ (Qk+l - Qkf M (q k+ i - q k ) - ~ (V(q k ) + V{q k+1 )) . 
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After some straightforward computations we obtain that equations (5a) and (5a) 
for the proposed nonholonomic discrete system are 

(9a) q k+1 - 2q k + q k -i = -h 2 M~ x (V q (q k ) + n T {q k )X k ) 

'qk+i - qk- 



(9b) = n(q h ) 



2h 



where X k are Lagrange multipliers. We recognize this set of equations as an obvious 
extension of the SHAKE method proposed by [26] to the case of nonholonomic 
constraints. 

The momentum is approximated by p k = M(q k +i—q k —i)/2h. Denoting Pk+1/2 = 
M(q k +i — q k )/h, equations (9a) and (9b) are now rewritten in the form 

Pk+1/2 =Pk-^; (V q (q k ) + H T (qk)X k ) , 

qk+i =qk + hM~ l p k+1 / 2 , 
= fiiq^M^pk. 

The definition of Pk+i requires the knowledge of qk+2 and, therefore, it is is 
natural to apply another step of the algorithm (9a) and (9b) to avoid this difficulty. 
Then, we obtain the new equations: 

Pk+i = Pk+1/2 - ^ {V q (q k +i) + M T (<7fc+i)Afc+i) , 
= fJ-{q k +i )M~ 1 p k+1 . 

The interesting result is that we obtain a natural extension of the RATTLE 
algorithm for holonomic systems to the case of nonholonomic systems. Unifying 
the equations above we obtain the following numerical scheme 

(10a) Pk+1/2 =Pk ~ I (V q (q k ) + H T (qk)Xk) , 

(10b) q k+ i =q k + fcM-^x/a, 

(10c) = /x(%)M"V, 

(10d) p k+1 = p k+ i/ 2 - | (V q (q k+1 ) + V T (q k +i)X k +i) , 

(10e) = fi(q k+1 )M- V+i- 

These equations allow us to take a triple (q k ,p k , X k ) satisfying the constraint equa- 
tions (10c), compute p k +i/2 using (10a) and then q k+ i using (10b). Then, equa- 
tions (10d) and (lOe) are used to compute the remaining components of the triple 
{q k +iiP k +i, \ k +i). It is clear, applying Theorem 1 that, in the case V = 0, the 
numerical method is energy preserving. 

Remark 1. From this Hamiltonian point of view, we have shown that the initial 
conditions for this numerical scheme are constrained in a natural way ((qo,po) with 
[i(qo)M~ 1 po = 0), that is, the initial conditions are exactly the same as those for 
the continuous system. Additionally, we select Ao = (see [12]). 

In [11], the following theorem is proven. 

Theorem 2. The nonholonomic RATTLE method is globally second-order conver- 
gent. 
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4.2. Projected Version of the Nonholonomic RATTLE. The proposed non- 
holonomic RATTLE method can be expressed without the use Lagrangian multipli- 
ers by projecting the equations of motion onto the constraint distribution through 
the projection V defined in §4. 

Assuming that the Lagrangian is regular and that matrix fj, is full rank (i.e. rank 
m) (9) can be reformulated as 

(11a) V{q k ) T M (q k+1 - 2q k + q k _i) = -h 2 T{q k ) T V q (q k ) 

(lib) g(g fc ) r A/( gfc+1 2fe gfc - 1 )=0, 

where the n x n matrices Q and V represent both orthogonal projectors and have 
rank m and (n — m), respectively, and are defined by 

(12a) Q{q) = M~ V(?) T (Kq)M~ V(?) T ) ~* ft{0), 

(12b) V(q)=U-Q(q), 

where Id is the identity matrix. 

Eqs. (11) correspond to (5) for the case Q = W 1 and furthermore can be put in 
the "momentum jump" form by adding (11a) and (lib) to get 

(13) q k+1 = q k + (ld-2i\/- 1 Q( (?fe ) T A/) (q k - q k ^) - h 2 M- l V{q k ) T V q {q k ). 

For a more realistic example, we can add control inputs u S U Cl c acting in the 
basis defined by the (n x c) matrix B{q) to obtain the following discrete equations: 

q k+1 = q k + (ld-2Af- 1 Q( (?fe ) T ^) (Qk - q k -l) + h 2 M- 1 V(q k ) T '/(#,«*), 

where the forces / : Q x U — > T*Q are given by f(q, u) = B(q)u — V q (q). 

In terms of momentum variables the integrator can be equivalently expressed as 

(14a) Pk+1/2 = (Id -2Q(q k ) T ) p k _ 1/2 + hV(q k ) T f(q k , u k ) 

(14b) q k+ i = q k + hM^ 1 p k+1/2 

providing an update scheme (qkiPk-i/z) (Qk+uPk+i/2)- 

A remaining critical step in completing the algorithm is to establish the link 
between the discrete variables (q k ,p k +i/2) IOT k = 0, N used in (14) and the con- 
tinuous curve (q(t),p(t)). In that respect one can regard p k = (pk-1/2 +Pfe+i/2)/2 
as an approximation to the continuous momentum at time t = kh, i.e. p k ~ p(kh). 
The pair (q k ,Pk) satisfies the nonholonomic constraint by definition and is related, 
following from (14), to the "midpoint" momenta through 

(15a) p k = V(q k ) T p k+1/2 - ^V(q k ) T f(q k ,u k ), 

(15b) Pk = V(q k ) T Pk _ 1/2 + ^T{q k ) T f(q k ,u k ). 

These expressions can be used to determine proper variables (91,^1/2) to initialize 
the update (14) given continuous initial conditions (qo,po) ~ (g(0),p(0)). Since 
there is a set of solutions P\i 2 satisfying (15) for a given po the most natural choice 
is to j>ickp 1 / 2 satisfying the constraints at qo- Therefore, the condition becomes 

Pl/2 =P0 + 7^(<7o) T f(qO, U ). 
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In summary, given initial conditions (qo,Po) satisfying the constraints, the dy- 
namics is evolved forwards to reach the final state (qn,Pn), also in the constraint 
submanifold, after N time steps through 



(16) 



P1/2 =po + ~V(qo) T f(qo, u ), 

Pk+1/2 = (Id -2Q(q k f) Pk - 1/2 + hP(q k ) T f(q k ,u k ), 

q k +i = q k + hAr 1 p k+1/2 , 

PN = V{q N ) T PN-l/2 + J -^P{q N )f{qN,u N ), 



for k = 1 N - 1. 



5. Reduced d'Alembert-Pontryagin integrator- (RDP) 

In this section we consider a class of mechanical systems which, in addition to 
nonholonomic constraints, also possess symmetries of motion arising from conser- 
vation laws. The interplay between the constraints and symmetries is linked to 
an intrinsic structure of the state space associated with important properties of 
the dynamics. Our goal in this section is to develop integrators that respect this 
structure and lead to more faithful numerical representation. 

In §4 we introduced the action of a symmetry group G and its relation the 
evolution of the system momentum. Additional structure arises whenever the dy- 
namics and constraints are G-invariant that permits the construction of reduced 
nonholonomic integrators [14, 17]. 

Following [1], define the subspaces V q and S q according to 

V, = {&(?) | £ e &}, S q = v q nv q . 

Practically speaking, the vertical space V q represents the space of tangent vectors 
parallel to symmetry directions while S q is the space of symmetry directions that 
satisfy the constraints. Equivalently, S q can be regarded as the space generated by 
elements in g 9 , as defined in (8). The group G is chosen so that the Lagrangian L 
and distribution D are G-invariant. In addition, we make the standard assumption 
(see [1, 7]) that T q Q = V q + V q , for each q 6 Q. 

Since our main interest is in a configuration space that is by construction of 
the form Q = M x G we will restrict any further derivations to the trivial bundle 
case. Using coordinates (r, g) G M x G a basis for g 9 can be chosen as {eb(r,g)}, 
for b = 1, dim(<S). Since V is G-invariant these elements can be expressed as 
e b(r,g) = Ad g efe(r), where {e&(r)} is the body-fixed basis. We denote g r the space 
spanned by {eb{r)} at each (r, e) £ Q. Lastly, the system is subject to control force 
/ : [0,T] -> T*M restricted to the shape space. 

Nonholonomic Connection. With these definitions we can define a principal connec- 
tion A : TQ — > q with horizontal distribution that coincides with H q at the point q, 
where T> q = S q © 1-L q . This connection is called the nonholonomic connection and is 
constructed according to A = A^ m + „4 sym , where A kln is the kinematic connection 
enforcing the nonholonomic constraints and ^4 sym is the mechanical connection 
corresponding to symmetries satisfying the constraints. These maps are defined 
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according to 

A kin (q) -q = 0. 

(17) V ' 

A sym (q)-q = Ad g n, 

where Q G g r is called the locked angular velocity, i.e. the velocity resulting from 
instantaneously locking the joints described by the variables r. Intuitively, when 
the joints stop moving the system continues its motion uniformly along a curve 
(with tangent vectors in S) with body-fixed velocity f2 and a corresponding spatial 
momentum that is conserved. 

By definition the principal connection can be expressed as 

A{q)-q = Ad g (g- 1 g + A(r)r), 

where A(r) is the local form and the two components in (17) can be added to obtain 

g- 1 g + A(r)r = n. 

Numerical Formulation. Since the Lagrangian is G-invariant, we can define the 
reduced Lagrangian £ : TM xg-it 

(18) e(r,r,0 = L(r,r,e,g- 1 g). 

In [17] a nonholonomic integrator was derived using a discrete variational d'Alembert- 
Pontryagin principle based on the reduced Lagrangian £, the connection A and a 
chosen trajectory discretization. In particular, a discrete trajectory with points 
qk = (r k ,g k ) £ M x G and respective velocities uu G TM and G g was con- 
structed so that 

rk+i - rk = hu k , T~ 1 (gf 1 g k+ i) = h£k, 

where £fc = Qk -A(r k+a )u kl with r k+a := (1 - a)r k + ar k+x for a chosen a G [0, 1]. 
The map r : g — > G represents the difference between two configurations in the 
group by an element in its algebra and can be selected as: 

• Exponential map exp : q — > G, defined by exp(£) = 7(1), with 7 : K — > G 
is the integral curve through the identity of the left invariant vector field 
associated with £ G (hence, with 7(0) = £); 

• Canonical coordinates of the second kind ccsk : g —> G, ccsk(£) = cxp(£ 1 ei)- 
cxp(£ 2 e2) • ... ■ cxp(£™e n ), where {ef\ is the Lie algebra basis. 

A third choice for r, valid only for certain quadratic matrix groups [6] (which 
include the rigid motion groups SO(3), SE(2), and SE(3)), is the Cayley map cay : 
->• G, cay(£) = (e - C/2)" 1 (e +£/2). (Sec App. A for more details). 

With these definitions in place the resulting reduced d'Alembert-Pontryagin 
(RDP) integrator can be stated [17]. For numerical convenience it is given in 
terms of vector-matrix notation, by treating the Lie algebra variables £ and fi as 
vectors of coordinates with respect to a chosen canonical basis (see App. B for an 
example). 

The discrete flow satisfies the reduced discrete dynamics 



(19) 



Id [A(r k )f 



[ ei (r fe ),...,e c (r fc )] J 



)- 


hfk 








<9„4 

where 4 := £(r k+a ,u k ,^ k ) and £ fc = fl k - A{r k+a )u k . The map dr c : g -> g 
is the right-trivialized tangent of r(£) defined by Dt(£) ■ 8 = TR T r£\(dT£ -5) and 
dr^ 1 : g — > g is its inverse (see App. A). 
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Equation (19) along with the reconstruction equations 
(20) g k+ i = gkr(h^k), r k+ i = r k + hu k , 

constitute the complete RDP discrete evolution. 



6. Examples 

6.1. The Chaplygin Sleigh. The Chaplygin Sleigh [1] is a planar rigid body 
making a contact with the ground through a skate mounted at the central axis of 
the body at a distance a from its center of mass (Fig. 1). The configuration space 
is the group G = SE(2) with coordinates q = (9, x, y) describing the orientation 
and the position of the center of mass. The body has rotational inertia I and mass 
m and, therefore, its Lagrangian is defined by 

(21) L{q,q) = \ie 2 + \m{x 2 +y 2 ). 




Figure 1. Chaplygin Sleigh model. 



At the point of the skate contact (x s ,y s ) = (x — a cos 8,y — a sin 6*) the body 
must slide in the direction in which it is pointing. This condition is encoded by the 
nonholonomic constraint 



ad + sin Ox — cos Oy = 0. 

A structure-preserving integrator was developed in [10] based on the discrete Lagrange- 
d'Alembert (DLA) principle with discrete momentum and measure preservation 
properties. Exploring this direction further, in this section we develop two alterna- 
tive methods based on the GNI and RDP schemes. 

GNI Integrator. From the mass matrix M = diag(/, m, m) and the constraint 
MiC?) = [a, sin 6*, — cos#], the projector Q can be computed using (12a) as 



(22) 



C(«) 



1 



/ + a 2 m 



al sin 6 

—al cos 6 



am sine/ 
7sin6> 2 
— /sin 8 cos ( 



—am cosd 
—Ismdcost 
I cos 2 8 



Since the mass matrix is constant, the GNI integrator can be derived according 
to v k+ i = (Id — M^ 1 Q(q k ) T M)v k _i . In terms of the coordinates v = (v e , v x , v v ), 
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the discrete update becomes 
2a 2 m 



am 



-2 sin 6kvf i+2 cos 6 k v 



Vfc+i = -^sin^_i + fl-^Bin 2 fc J + ^sin^cos^ ,, 

<+i = ^cos^_i +y sin0 fc cos0 fc ^_i + fl-^cos 2 ^ 

where I' = I + a 2 m. It is straightforward to verify that the resulting update rule 
is energy-preserving, i.e. (Md m/2 , v k _x/ 2 ) = {Mv k+1 / 2 ,v k+1/2 ). This property is 
inherent to the GNI construction as explained in [12]. 

RDP Integrator. The sleigh has no internal joints and therefore no shape space. 
Since the Lagrangian (21) is left-invariant to SE(2) group action, the reduced La- 
grangian (18) can be expressed as 

e(0 = L(e,g- 1 g), 

where £ = (cj, v, v 1 ) £ g describes the angular, forward, and sideways velocities with 
respect to the body frame fixed at the center of mass. The constrained symmetry 
space (8) of the sleigh can be identified as 

g q = span{ei( 5 ),e 2 (#)}, 

where e\ — (1,0, a) £ g and e 2 — (0,1,0) £ g form the constant basis in the 
body-fixed frame with ei(g) = Ad^e^, for i = 1,2. The two components of the 
nonholonomic momentum pi = (d^£, ei) become 

pi = (J + a 2 m)u}, p 2 = mv, 

corresponding to angular and forward momenta, respectively. The group trajectory 
can be reconstructed from the momentum according to 



-i 



1 1 



\l+a z m m 1+am 

The momentum components themselves evolve according to ^ = (aAtd^i, ei) (see [2]), 
or equivalcntly 

a ma 2 

Pi = -7- — — P1P2, P2 = 77- — 2-T2PV 
1 + a z m (i + a z m) z 

Since the shape space consists of a single point, the discrete dynamics includes 
only the momentum equations (19) which become 

<(dr^)*^4 - (drl^r^-i,^) = 0, 

for i = l,2. A simple form of these equations can be derived by choosing r = cxp 
and truncating its tangent to first order, i.e. dr^T 1 ss Id — ^ad^. Using the notation 
Pk = ((Pi)k, (P2)k) the update becomes 



ha 

2(J~ 2 m) 



(Pi)k - (pi)fe-i = -7771- — 2~^T [{Pl)k{P2)k + {Pl)k-l{P2)k- 



(P2)k - (Pa)ifc-i = 2 (J+™2 m )2 ^ J ) fc2 + (Pi)fc-i 2 
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These conditions are used to solve for the unknown next momentum pk, e.g. through 
cubic equation root-finding. Note, that this particular choice of approximation ex- 
actly matches a standard implicit central difference discretization of the continuous 
ODE. This is generally not case for systems with non-constant Lie algebra basis 
element such as the snakeboard. Higher accuracy can be achieved through other 
choices of r and better approximation of dr. App. B details the cases r = exp and 
t = cay on SE(2). 

The reconstruction equations are 

9k+i = g k exp(h£, k ), 



where 




X 



Figure 2. Position curves (left) of the sleigh integrators and the 
corresponding energy (right). The embedded close-up frame (left) 
zooms in on the cusp point of the "heart" shape. 

Numerical Comparisons. The numerical behavior of the algorithms is now examined 
in terms of their ability to reproduce the true system trajectory and in terms of 
their energy preservation. Comparison to a standard Rungc-Kutta second-order 
method is also included. 

Note that the standard Chaplygin sleigh model (e.g. [1, 10, 12]) is studied in 
terms of the coordinates of the skate contact rather than the center off mass as in 
this work. For easier reference to such previous studies, we present the position 
curves below in terms of the skate coordinates (x s , y s )- This representation enables 
the generation of the familiar "heart" -shaped curves (Fig. 2). 

6.2. The Snakeboard. The snakeboard (Fig. 3) represents a type of system with 
an interesting interplay between constraints and symmetries. It has served as a 
classical example (e.g. [2, 7, 4]) of a system with non-trivial intersection of the 
constraint distribution T> and the vertical space V. Our integrators capture the 
dynamics of such systems and their performance is examined in this section. 

The shape space variables of the snakeboard are r = (?/>, <f) £ S 1 x S 1 denoting 
the rotor angle and the steering wheels angle, while its configuration is defined 
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Figure 3. Snakcboard model (left) and a typical trajectory (right). 



by (6, x, y) denoting orientation and position of the board. This corresponds to a 
configuration space Q = S 1 x S 1 x SE(2) with shape space M — S 1 x S 1 and group 
G = SE(2). Additional parameters are its mass m, distance I from its center to 
the wheels, and moments of inertia I and J of the board and the steering. The 
kinematic constraints of the snakeboard are: 

— I cos 4>d6 — sin(# + <ft)dx + cos(9 + 4>)dy = 0, 



I cos 



sin 



4>)dx + cos(# — 4>)dy = 0, 



enforcing the fact that the system must move in the direction in which the wheels arc 
pointing and spinning. The constraint distribution is spanned by three covectors: 

8 8 8 



T> q = span 



d , d 
ox oy 



where a = —21 cos cos 2 <fi, b = — 21 sin 6* cos 2 <j>, c = sin 20. The group directions 
defining the vertical space are: 

d d d 
86' dx' dy 

and therefore the constrained symmetry space becomes: 



V a 



span 



(24) 



S q = V q (~l V q = span < c— + a— — h b— 



8 



d 



86 dx 



dy 



Since V q = S q 



% q , we have Ti. q = span j ^ , ^ | . Finally, the Lagrangian of the 



system is L(q,q) = ^q T Mq where 



M 





2.7 






/ 


ml 2 








m 









m 



The reduced Lagrangian can be expressed as £(r, u, ^) = (u, ^) T M (u, ^) by treating 
the velocity £ as a vector in the standard se(2) basis (defined in App. B). 

There is only one direction along which snakcboard motions lead to momentum 
conservation: it is defined by the basis element 



ei(r) = 21 cos 2 cj) 



tan (p 
-1 
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and, hence, there is only one momentum variable p\ = 
variable we can derive the connection according to [2] as 

,2 i r 



[A} = 



■ sin 







, and fl = 



Pi 



4ml 2 cos 2 



ei(r)V Using this 



rei(r). 



GNI Integrator. The snakcboard constraints (23) can be expressed in terms of the 
one-forms 

fixiq) = (0,0,a,-c,0), fi 2 {q) = (0, 0, 6, 0, -c). 

The projector Q can then be computed from fi and the mass matrix M using (12a) 
to obtain 



1 



/ 




ml 2 — I sin" i 





\ 



-m(a 2 + b 2 ) 


m(a 2 + b 2 ) 
-Vac 
-I'bc 



mac 


—mac 
mb 2 + Vc 2 
—mab 



mbc 


—mbc 
—mab 



\ 



where V = ml 2 — I and q = (i/),<f),9,x,y). Similarly to the Chaplygin sleigh §6.1, 
since the mass matrix is constant, the discrete dynamics is updated explicitly 
through v k+ i = (Id - M~ 1 Q(q k ) T M)v k _i. 

RDP Integrator. The reduced discrete equations of motion will be derived by substi- 
tuting the Lagrangian and the connection of the snakeboard into (19) and choosing 
the map r = exp. Since, particularly for the snakeboard, s is one dimensional 
and A(r) ■ 5 is parallel to ei(r) for any S £ T r M the discrete dynamics simplifies 
(see [17, 16]) to 



(p k -Pfc-i,ei(r fc )) = 0, 



k+a 



fe-i+ 



a = 0, 



where 



Pk = (mi 2 e k +iutme k ,o), dj k = (i(uf+e k ),2jui), 



and the dynamics is derived by expressing £ k = fl k — A(r k+a ) ■ u k in terms of 
r k = (ip k ,(f) k ), u k = (uf,uf.), and (pi)fc. Note that the discrete dynamics is linear 
in the unknowns u k and (pi) k and results in an efficient explicit integrator. The 
reconstruction equations are 

9k+i = 9k exp(/i£ fc ), r k+1 -r k = hu k . 
Numerical Behavior. The studied snakeboard integrators are second-order methods. 
Their advantage over similar methods is shown through comparison to a typical 
second order Runge-Kutta method as well as to the actual true trajectory. Fig. 4 
shows a trajectory with initial conditions ip(0) = 7r/2, (f>(0) = 7r/3, pi(0) = — 1, 
ijj(0) = 2.5, 0(0) = -0.02, 6»(0) = 0. Sinusoidal control inputs u, p = cos(207rt), 
= sin(27rt) at the joints were used to create parallel parking maneuvers with 
cusp points. The CPU run-times of the compared methods are nearly identical and 
are not included in the plots. 

In special cases, for particular combinations of initial conditions and inertial 
parameters, the GNI integrator has shown non-physical oscillatory behavior. While 
this issue is most likely related to instabilities known to occur in projection-based 
methods, the exact cause remains to be determined in future work. 
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Position Curve Energy Error 




sec 



Figure 4. Snakcboard integrator numerics with N = 128 
timcsteps over the integration horizon T = 10 sec. At such coarse 
resolution RK2 method fails to follow the true trajectory while 
GNI and RDP have qualitatively correct behavior (position curve 
on left). One likely explanation lies in their better energy behavior 
(shown on right). 



7. Conclusion 

In this paper we have compared two geometric integrators for nonholonomic 
dynamics, the so-called GNI and RDP integrators. Both are constructed using dif- 
ferential geometric tools developed by geometric mechanics community through a 
careful study of nonholonomic dynamics during the last twenty years. This paper 
shows the importance of combining different research areas (differential geometry, 
numerical analysis and mechanics) to produce methods with an extraordinary qual- 
itative and quantitative behavior. 

Such issues raise a number of future work directions. We therefore close with 
some open questions: 

• Given one of the nonholonomic integrators (GNI or RDP), does there exist, 
in the sense of backward error analysis, a continuous nonholonomic system, 
such that the discrete evolution for the nonholonomic integrator is the flow 
of this nonholonomic system up to an appropriate order? 

• Is it possible to use the nonholonomic Hamilton- Jacobi theory recently 
developed [15, 19] for the construction of these methods or new ones? 

These questions will be part of the future work that we will develop in the next 
years. 



Appendix 

Appendix A. Retraction map tangents 

The two common choices for retraction maps are the exponential map r = exp 
and the Cayley map r = cay. In this section we provide their right-trivialized 
tangents dr of these maps and their inverses dr _1 (see [3] for more details). 
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A.l. Exponential map. The right-trivialized derivative of the map exp and its 

inverse arc defined as 



(25) 



CO ^ OC j ^ 

dcxp x y = Yl (7+1)! ^ y ' dexp ^ 1 y = ~~jf ad * 



y, 



j=0 w ' 3=0 

where Bj are the Bernoulli numbers. Typically, these expressions are truncated in 
order to achieve a desired order of accuracy. The first few Bernoulli numbers are 
B = 1, Bt = -1/2, B 2 = 1/6, B 3 = (see [6, 13] for more details). 

A. 2. Cayley map. The derivative maps become (see [13] for derivation) 
(26) dca Yx y= \y(l+f) \ dcay^ 1 y = y 

Appendix B. Retraction Maps on SE(2) 
The coordinates of SE(2) are (8, x, y) with matrix representation g G SE(2) given 



by: 

(27) g 
Using the isomorphic map ~ : 



cos — sin 8 x 
sin 8 cos 6* y 
1 

-5- se(2) given by: 








-v 1 


w 2 " 






u = 









for t' = 




















{ei, e2,e"3} can be used as a basis for se(2), where {ei, e2, 63} is the standard basis 

of m 3 . 

The two maps r : se(2) — > SE(2) are given by 



cxp(i)) = < 



cay{v) ■- 



1 1 v 2 sin v 1 —v 3 (1— cos v 1 ) 

COSV —smv r 

1 1 v 2 (1 — cos i; 1 )+d 3 sin v 1 

sm f cos v — 1 P- 

1 

1 v 2 
1 v 3 
1 

( v i)2_4 _4 w i 

4U 1 (u 1 ) 2 -4 




4+(w 1 ) 2 



if v 1 ± 
if v 1 = 



-2w 1 w 3 + 4t> 2 
2u 1 u 2 +4u 3 
1 



The maps [dr^ ] can be expressed as the 3x3 matrices: 
(28) [dexpr^Ig-I^l + ^Iad,,] 2 , 



(29) 
where 



[dcay^ 1 ] = I 3 - -[ad„] + - [ v 1 ■ v 3x2 ] , 
[ad„] = 



-v 
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